/**
 * sukai
 * 大小端转换工具
 */
#include "utils.h"
#include <math.h>
#include <memory.h>
#include <QDebug>


void reverseByte(char *p, int size) {
    if (size == 4) {
        //0001
        char tmp = p[0];//*p
        p[0] = p[3];//*(p+3)
        p[3] = tmp;

        tmp = p[1];
        p[1] = p[2];
        p[2] = tmp;
    } else if (size == 8) {
        //10000000
        char tmp = p[0];
        p[0] = p[7];
        p[7] = tmp;


        tmp = p[1];
        p[1] = p[6];
        p[6] = tmp;

        tmp = p[2];
        p[2] = p[5];
        p[5] = tmp;

        tmp = p[3];
        p[3] = p[4];
        p[4] = tmp;
    }
}
//isClose_resultVariance:目标精度 定义在/src/driver/IIMTDriver.h;  ，resultVariance：获取计算返回的精度数据方便调试
//sukai bool isClose(Instruction &target, IIMT_Data &iimtData,double &isClose_resultVariance,double &resultVariance) {
    bool isClose(Instruction &target, IIMT_Data &iimtData,double &isClose_resultVariance) {

        //目标数据
    double targetData[6];
    memcpy(targetData, target.data, 6 * sizeof(double));
    //当前数据
    double ciimtData[6];
    if (target.movetype == MOVEJ) {
        memcpy(ciimtData, iimtData.q_actual, 6 * sizeof(double));
    } else if (target.movetype == MOVEL || target.movetype == MOVEP) {
        memcpy(ciimtData, iimtData.Tool_vector_actual, 6 * sizeof(double));
    }else if (target.movetype == MOVEC) {

    }
    //判断是否足够接近
    double result = variance(target.movetype, targetData, ciimtData);
   // qDebug()<<"===============result/6=================: "<<(result/6);
    return result < isClose_resultVariance;
}

double variance(MOVETYPE movetype, double *targetData, double *ciimtData) {
    double result;
    for (int i = 0; i < 6; ++i) {
        if (movetype == MOVEL && (i == 3 || i == 4 || i == 5)) {
            result += pow(abs(targetData[i]) - abs(ciimtData[i]), 2);
        }else if (movetype == MOVEP && (i == 3 || i == 4 || i == 5)) {
            result += pow(abs(targetData[i]) - abs(ciimtData[i]), 2);
        } else {
            result += pow(targetData[i] - ciimtData[i], 2);
        }
        // qDebug()<<"targetData[i]: "<<targetData[i];
        //qDebug()<<"ciimtData[i]: "<<ciimtData[i];
    }

    // qDebug()<<"result/6: "<<result/6;
    return result / 6;
}


